#include "carvingmodule.h"
#include <QDebug>
#include "mot_priv.h"
#include <unistd.h>
#include <sys/syscall.h>
#include "dbuf.h"
#define gettid() syscall(__NR_gettid)
#include <stdio.h>
#include "posemath.h"
#include "rtapi.h"
#include "hal.h"
#include "motion.h"
#include "mot_priv.h"
#include "math.h"
#include "tp.h"
//#include "tc.h"
#include "simple_tp.h"
#include "motion_debug.h"
#include "config.h"
#include "motion_types.h"
#include "cubicclass.h"
#include "tcclass.h"

#include "emccommandclass.h"
#include "emccontrolclass.h"
#include "emcmotionclass.h"
#include "coordinatemanager.h"

EmcDebugInfo g_emcDebugInfo[D_MAX_CARVE_NUM];

#define D_JOINT_VEL_LIMIT_RATIO 0.95

/***********************************************************************
*                   GLOBAL VARIABLE DECLARATIONS                       *
************************************************************************/

///* HAL component ID for motion module */
//// int mot_comp_id;

///* userdefined number of joints. default is EMCMOT_MAX_JOINTS(=8),
//   but can be altered at motmod insmod time */
// int num_joints;

///* userdefined number of digital IO. default is 4. (EMCMOT_MAX_DIO=64),
//   but can be altered at motmod insmod time */
// int num_dio;

///* userdefined number of analog IO. default is 4. (EMCMOT_MAX_AIO=64),
//   but can be altered at motmod insmod time */
// int num_aio;

///* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */
// emcmot_hal_data_t *emcmot_hal_data;

///* pointer to array of joint structs with all joint data */
///* the actual array may be in shared memory or in kernel space, as
//   determined by the init code in motion.c
//*/
// emcmot_joint_t *joints;

///* flag used to indicate that this is the very first pass thru the
//   code.  Various places in the code use this to set initial conditions
//   and avoid startup glitches.
//*/
// int first_pass;

///* Variable defs */
// int kinType;
// int rehomeAll;
//// int DEBUG_MOTION;
// int EMCMOT_NO_FORWARD_KINEMATICS;
// KINEMATICS_FORWARD_FLAGS fflags;
// KINEMATICS_INVERSE_FLAGS iflags;
///* these variables have the servo cycle time and 1/cycle time */
// double servo_period;
// double servo_freq;


///* Struct pointers */
// struct emcmot_struct_t *emcmotStruct;
// struct emcmot_command_t *emcmotCommand;
// struct emcmot_status_t *emcmotStatus[robotIdIn];
// struct emcmot_config_t *emcmotConfig;
// struct emcmot_debug_t *emcmotDebug[robotIdIn];
// struct emcmot_internal_t *emcmotInternal;
// struct emcmot_error_t *emcmotError;

// TP_STRUCT *emcmotPrimQueue;
// TP_STRUCT *emcmotAltQueue;
// TP_STRUCT *emcmotQueue[robotIdIn];

// long traj_period_nsec; // motion.c commandline argument


CarvingModule::CarvingModule(RobotCommonParameter * roboParameterIn, CoordinateManager *coordinateManagerIn
                             , MotionMessage *motionMessageIn)
{

    coordinateManager=coordinateManagerIn;
    robotId=roboParameterIn->robotId;
    struct emcmot_config_t motConfig;
    //轨迹插补时间可能为０．０１ｓ
    motConfig.trajCycleTime=roboParameterIn->deltaTime;
    motConfig.arcBlendGapCycles=3;
    motConfig.arcBlendOptDepth=3;//速度优化深度
    motConfig.arcBlendEnable=1;//是否允许cnt平滑过渡
    motConfig.arcBlendRampFreq=1;
    //0.001- 0.7071;影响选择parabolic抛物线插值还是 切向arcblend过渡。越大越倾向加圆弧arcblend过渡　　tpCheckTangentPerformance
    //tpCreateLineLineBlend ,tpComputeOptimalVelocity一直会优化改变速度
    motConfig.arcBlendTangentKinkRatio=0.01;
    motConfig.arcBlendFallbackEnable=0;
    motConfig.maxFeedScale=10;//没起作用
    //joint->vel_limit = 1171
    //如果关节速度超过　限制速度，则单个关节的速度降低到百分比arm_warnSlowRatio，如果是直线，轨迹会扭曲。
    emcControl=new EmcControlClass(this,roboParameterIn->arm_warnSlowRatio,roboParameterIn->deltaTime
                 ,motionMessageIn,roboParameterIn->lineMaxSpeed,roboParameterIn->lineMaxAcceleration
                                   ,robotId);

    EmcMotionClass::rtapi_app_main_intial(motConfig,robotId);
//    EmcMotionClass::emcmotSetCycleTime(8000000);
    //轨迹周期必须大于等于伺服周期
    EmcMotionClass::setTrajCycleTime(roboParameterIn->deltaTime,robotId);
    EmcMotionClass::setServoCycleTime(roboParameterIn->deltaTime,robotId);
    //addLine时限制加速度。tc->maxaccel = acc;　tc->progress长度，并不是时间。
    EmcCommandClass::emcSetVelLimit(roboParameterIn->lineMaxSpeed,robotId);//只有速度限制，没有加速度限制。变量定义了，没有使用
    EmcCommandClass::emcSetRotateLimit(roboParameterIn->rotate_eqradius,robotId);
    for(int i=0;i<roboParameterIn->jointAttributeVector.size();i++)
    {
        EmcMotionClass::setJointVel(i,roboParameterIn->jointAttributeVector[i].maxVelocity,
                                    roboParameterIn->jointAttributeVector[i].maxAccelation,robotId);//
        //todo bound vel acc单独设置
        EmcMotionClass::setBoundVel(i,roboParameterIn->jointAttributeVector[i].maxVelocity,
                                    roboParameterIn->jointAttributeVector[i].maxAccelation,robotId);
    }




//     test1();
    emcControl->set_operating_mode();
//    createThread();

}

int CarvingModule::resetAll()
{
    tpTrajMutex.lock();
    int  tmpKey=EmcCommandClass::emcTpClear(robotId);
    emcControl->resetAll();
    tpTrajMutex.unlock();
    return tmpKey;
}

E_WAVE_STATUS CarvingModule::getWaveMotionStatus()
{
    return emcControl->getWaveMotionStatus();
}

int CarvingModule::setTimeAxisScale(double ratio)
{
    return emcControl->setTimeAxisScale(ratio);
}

int CarvingModule::startWave()
{
    return emcControl->startWave();
}

int CarvingModule::stopWave()
{
    return emcControl->stopWave();
}

int CarvingModule::ceaseWave()
{
    return emcControl->ceaseWave();
}

int CarvingModule::setWaveType(E_WAVE_TYPE typeIn, double waveWidthIn, double wavePeriodIn)
{
    return emcControl->setWaveType(typeIn,waveWidthIn,wavePeriodIn);
}

int CarvingModule::setWaveMotionInfo(WeaveMotionCondition infoIn)
{
    return emcControl->setWaveMotionInfo(infoIn);
}

int CarvingModule::setWaveAcceleration(double accelerationIn,double maxVelIn,bool isRatio)
{
    return emcControl->setWaveAcceleration(accelerationIn,maxVelIn,isRatio);
}

int CarvingModule::setNetFeedScale(double scaleIn)
{
    return emcControl->setNetFeedScale(scaleIn);
}

int CarvingModule::setJointVel(int jointNum,double maxVel,double maxAcc)
{
//    tpTrajMutex.lock();
    int tmpKey=EmcMotionClass::setJointVel(jointNum,maxVel,maxAcc,robotId);
//    tpTrajMutex.unlock();

    return tmpKey;
}



int CarvingModule::getEmcDebugInfo(EmcDebugInfo &debugOut)
{
    //emcmotQueue[robotIdIn]
    debugOut=g_emcDebugInfo[robotId];
    return 1;
}

int CarvingModule::getTpQueueSize()
{
    return g_emcDebugInfo[robotId].tpInfo.queueSize;
}

int CarvingModule::loopRealTimeOnce()
{
    tpTrajMutex.lock();
    int tmpKey=emcControl->emcmotController_test();
    tpTrajMutex.unlock();

    return tmpKey;
}

int CarvingModule::getJointCommand(std::vector<double> &jointCommandOut)
{
    return emcControl->getJointCommand(jointCommandOut);
}

int CarvingModule::getCoarsePositionCommand(std::vector<double> &coarsePointOut)
{
    tpTrajMutex.lock();
    coarsePointOut.resize(9);
    coarsePointOut[0]=emcmotStatus[robotId]->carte_pos_cmd.tran.x;
    coarsePointOut[1]=emcmotStatus[robotId]->carte_pos_cmd.tran.y;
    coarsePointOut[2]=emcmotStatus[robotId]->carte_pos_cmd.tran.z;
    coarsePointOut[3]=emcmotStatus[robotId]->carte_pos_cmd.a;
    coarsePointOut[4]=emcmotStatus[robotId]->carte_pos_cmd.b;
    coarsePointOut[5]=emcmotStatus[robotId]->carte_pos_cmd.c;
    coarsePointOut[6]=emcmotStatus[robotId]->carte_pos_cmd.u;
    coarsePointOut[7]=emcmotStatus[robotId]->carte_pos_cmd.v;
    coarsePointOut[8]=emcmotStatus[robotId]->carte_pos_cmd.w;
    tpTrajMutex.unlock();
    return 1;

}

int CarvingModule::resetCurrentPosition(std::vector<double> &pointIn, std::vector<double> &jointsIn)
{
    EmcPose pos;
    ZERO_EMC_POSE(pos);
    pos.tran.x=pointIn[0];
    pos.tran.y=pointIn[1];
    pos.tran.z=pointIn[2];
    pos.a=pointIn[3];
    pos.b=pointIn[4];
    pos.c=pointIn[5];
    tpTrajMutex.lock();
//    emcControl->setNetFeedScale(1);
    emcControl->resetJointsCommand(jointsIn);
    int tmpKey= tpSetPos(emcmotQueue[robotId], &pos);
    tpTrajMutex.unlock();
    return tmpKey;
}



double CarvingModule::getRunningNumber()
{
    if(0==emcmotStatus[robotId]->depth)
    {
        return emcmotStatus[robotId]->id+0.9;
    }
    return emcmotStatus[robotId]->id+0.5;

}

int CarvingModule::setRunningNumber_robotArmControl(double numberIn)
{
    tpTrajMutex.lock();
    tpSetExecId(&emcmotDebug[robotId]->tp,numberIn);
    emcmotStatus[robotId]->id=numberIn-1;
    tpTrajMutex.unlock();
    return 1;
}

/* Find the real destination, given the axis's current position, the
   commanded destination, and the direction to turn (which comes from
   the sign of the commanded value in the gcode).  Modulo 360 positions
   of the axis are considered equivalent and we just need to find the
   nearest one. */

//int Interp::unwrap_rotary(double *r, double sign_of, double commanded, double current, char axis) {
//    double result;
//    int neg = copysign(1.0, sign_of) < 0.0;
//    CHKS((sign_of <= -360.0 || sign_of >= 360.0), (_("Invalid absolute position %5.2f for wrapped rotary axis %c")), sign_of, axis);

//    double d = floor(current/360.0);
//    result = fabs(commanded) + (d*360.0);
//    if(!neg && result < current) result += 360.0;
//    if(neg && result > current) result -= 360.0;
//    *r = result;

//    return INTERP_OK;
//}


int CarvingModule::addLine(int idIn,double x, double y, double z, double w, double p, double r,
                           double vel, double acc)
{
    EmcPose pos;
    ZERO_EMC_POSE(pos);
    pos.tran.x=x;
    pos.tran.y=y;
    pos.tran.z=z;
    pos.a=w;
    pos.b=p;
    pos.c=r;

    emcmotCommand[robotId]->pos=pos;
    emcmotCommand[robotId]->id=idIn;
    emcmotCommand[robotId]->motion_type=EMC_MOTION_TYPE_FEED;
    emcmotCommand[robotId]->vel=vel;
    emcmotCommand[robotId]->ini_maxvel=vel;
    emcmotCommand[robotId]->acc=acc;//线加速度
    emcmotCommand[robotId]->turn=5;//必须大于2
    emcmotCommand[robotId]->tag.feed=50;
    emcmotCommand[robotId]->tag.speed=8;
    tpTrajMutex.lock();
    int tmpKey=EmcCommandClass::emcMotSetLine(robotId);
    tpTrajMutex.unlock();
    fflush(stdout);
    return tmpKey;
}

int CarvingModule::addLine(int idIn, std::vector<double> posIn, double vel, double acc)
{
    if(posIn.size()<9)
    {
        qDebug()<<"posIn.size()<9"<<posIn.size();
        return 0;
    }
    for(int i=0;i<posIn.size();i++)
    {
        if(isnan(posIn[i]))
        {
            qDebug()<<"error,CarvingModule::addLine,,isnan(posIn[i]) i="<<i;
            return 0;
        }
    }
    EmcPose pos;
    ZERO_EMC_POSE(pos);
    pos.tran.x=posIn[0];
    pos.tran.y=posIn[1];
    pos.tran.z=posIn[2];
    pos.a=posIn[3];
    pos.b=posIn[4];
    pos.c=posIn[5];
    pos.u=posIn[6];
    pos.v=posIn[7];
    pos.w=posIn[8];

    emcmotCommand[robotId]->pos=pos;
    emcmotCommand[robotId]->id=idIn;
    emcmotCommand[robotId]->motion_type=EMC_MOTION_TYPE_FEED;
    emcmotCommand[robotId]->vel=vel;
    emcmotCommand[robotId]->ini_maxvel=vel;
    emcmotCommand[robotId]->acc=acc;
    emcmotCommand[robotId]->turn=5;//必须大于2
    emcmotCommand[robotId]->tag.feed=50;
    emcmotCommand[robotId]->tag.speed=8;

    tpTrajMutex.lock();
    int tmpKey=EmcCommandClass::emcMotSetLine(robotId);//新加的命令没生效？
    tpTrajMutex.unlock();
    fflush(stdout);
    return tmpKey;

}



int CarvingModule::addCircle(int idIn, std::vector<double> posIn,
                             std::vector<double> centerIn, std::vector<double> normalIn,
                             double vel, double acc,int turnsIn)
{
    qDebug()<<"addCircle"<<idIn;
    if(posIn.size()<9)
    {
        qDebug()<<"posIn.size()<9"<<posIn.size();
        return 0;
    }
    if(centerIn.size()<3)
    {
        qDebug()<<"centerIn.size()<3"<<centerIn.size();
        return 0;
    }
    if(normalIn.size()<3)
    {
        qDebug()<<"normalIn.size()<9"<<normalIn.size();
        return 0;
    }
    for(int i=0;i<posIn.size();i++)
    {
        if(isnan(posIn[i]))
        {
            qDebug()<<"error,CarvingModule::addCircle,,isnan(posIn[i]) i="<<i;
            return 0;
        }
    }
    EmcPose pos;
    ZERO_EMC_POSE(pos);
    pos.tran.x=posIn[0];
    pos.tran.y=posIn[1];
    pos.tran.z=posIn[2];
    pos.a=posIn[3];
    pos.b=posIn[4];
    pos.c=posIn[5];
    pos.u=posIn[6];
    pos.v=posIn[7];
    pos.w=posIn[8];

    emcmotCommand[robotId]->pos=pos;
    emcmotCommand[robotId]->id=idIn;
    emcmotCommand[robotId]->motion_type=EMC_MOTION_TYPE_FEED;//EMC_MOTION_TYPE_ARC
    emcmotCommand[robotId]->vel=vel;
    emcmotCommand[robotId]->ini_maxvel=vel;
    emcmotCommand[robotId]->acc=acc;
    emcmotCommand[robotId]->turn=turnsIn;//必须>=0
    emcmotCommand[robotId]->tag.feed=50;
    emcmotCommand[robotId]->tag.speed=8;

   emcmotCommand[robotId]->center.x=centerIn[0];//圆心
   emcmotCommand[robotId]->center.y=centerIn[1];
   emcmotCommand[robotId]->center.z=centerIn[2];
   emcmotCommand[robotId]->normal.x=normalIn[0];//向量
   emcmotCommand[robotId]->normal.y=normalIn[1];
   emcmotCommand[robotId]->normal.z=normalIn[2];


    tpTrajMutex.lock();
    int tmpKey=EmcCommandClass::emcMotSetCircle(robotId);
    tpTrajMutex.unlock();
    fflush(stdout);
    return tmpKey;
}

int CarvingModule::emcTpPause()
{
//    qDebug()<<"CarvingModule::emcTpPause()";
    tpTrajMutex.lock();
    int  tmpKey=EmcCommandClass::emcTpPause(robotId);
    tpTrajMutex.unlock();
    ceaseWave();
    return tmpKey;
}

int CarvingModule::emcTpResume()
{
    tpTrajMutex.lock();
    int  tmpKey=EmcCommandClass::emcTpResume(robotId);
    tpTrajMutex.unlock();
    startWave();
    return tmpKey;
}

int CarvingModule::emcTpClear()
{
//    qDebug()<<"CarvingModule::emcTpClear";
    tpTrajMutex.lock();
    int  tmpKey=EmcCommandClass::emcTpClear(robotId);
    tpTrajMutex.unlock();
    return tmpKey;
}

int CarvingModule::emcSetVelLimit(double maxLineVelIn)
{
    tpTrajMutex.lock();
    int  tmpKey=EmcCommandClass::emcSetVelLimit(maxLineVelIn,robotId);
    tpTrajMutex.unlock();
    return tmpKey;

}

int CarvingModule::emcSetRotateLimit(double rotate_eqradiusIn)
{
    tpTrajMutex.lock();
    int  tmpKey=EmcCommandClass::emcSetRotateLimit(rotate_eqradiusIn,robotId);
    tpTrajMutex.unlock();
    return tmpKey;
}

int CarvingModule::kinematicsInverse(std::vector<double> posIn, std::vector<double> initialJointIn,
                                     std::vector<double> &jointOut, bool isErrorNotice)
{

    int tmpKey= coordinateManager->kinematicsInverse(posIn,initialJointIn,jointOut,isErrorNotice);
//    qDebug()<<"CarvingModule::kinematicsInverse posIn"<<QVector<double>::fromStdVector( posIn)
//              <<"initialJointIn"<<QVector<double>::fromStdVector( initialJointIn)
//                <<"jointOut"<<QVector<double>::fromStdVector( jointOut);
    return tmpKey;
}

void CarvingModule::test1()
{



}


void* CarvingModule::threadRun(void *)
{
    printf("SafetyFunction theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 50;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************threadRun1 pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);

    while(1)
    {
        emcControl->emcmotController_test();
        fflush(stdout);
        usleep(1000);
    }


    printf("GrblAdapter thread quit!!!");
}


void* CarvingModule::threadTask( void* classPtr )
{
    qDebug()<<"GrblAdapter thread run-------------------------";
    return ((CarvingModule*)classPtr)->threadRun(NULL);
}

void CarvingModule::createThread()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &CarvingModule::threadTask,this);
    qDebug()<<"GrblAdapter createThread-------------------------";
}


void* CarvingModule::threadRun2(void *)
{
    printf(" theread2 start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 50;
    sched_setscheduler(0,SCHED_FIFO,&param);
    qDebug()<<"************************threadRun2 pid="<<gettid();
    usleep(659000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);

    while(1)
    {
//        stepper_period_call();
        long period=1;
        int arg=1;
//        emcmotCommandHandler(&arg, period);
//        emcmotController(&arg, period);
        usleep(1000);//hualei
    }

    printf("GrblAdapter thread２ quit!!!");
}

void* CarvingModule::threadTask2( void* classPtr )
{
    qDebug()<<"GrblAdapter thread2 run-------------------------";
    return ((CarvingModule*)classPtr)->threadRun2(NULL);
}

void CarvingModule::createThread2()
{
    pthread_t threadId;
    int ret=pthread_create( &threadId, NULL, &CarvingModule::threadTask2,this);
    qDebug()<<"GrblAdapter createThread2-------------------------";
}
